#include "laserMapping.h"

#include "optimize/mapOptmization.hpp"

int main(int argc, char** argv) {
    ros::init(argc, argv, "laserMapping");
    ros::NodeHandle nh("~"); //使用私有的 NodeHandle,它会在发布或订阅的话题前自动加上节点的名称作为前缀。
    // ros::NodeHandle nh;
    readParameters(nh);
    LoadGTSAM(nh);
    // 回环检测线程
    std::thread loopthread(&loopClosureThread);

    cout << "lidar_type: " << lidar_type << endl;

    path.header.stamp = ros::Time().fromSec(lidar_end_time);
    path.header.frame_id = "camera_init";

    /*** variables definition for counting ***/
    int frame_num = 0;
    double aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_propag = 0;
    std::time_t startTime, endTime;

    /*** initialize variables ***/
    double FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0);
    double HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0);

    memset(point_selected_surf, true, sizeof(point_selected_surf));
    downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
    downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);
    Lidar_T_wrt_IMU << VEC_FROM_ARRAY(extrinT);
    Lidar_R_wrt_IMU << MAT_FROM_ARRAY(extrinR);

    p_imu->lidar_type = p_pre->lidar_type = lidar_type;
    p_imu->imu_en = imu_en;

    kf_output.init_dyn_share_modified_2h(get_f_output, df_dx_output, h_model_output, h_model_IMU_output);
    Eigen::Matrix<double, 30, 30> P_init_output = MD(30, 30)::Identity() * 0.01;
    P_init_output.block<3, 3>(21, 21) = MD(3, 3)::Identity() * 0.0001;
    P_init_output.block<6, 6>(6, 6) = MD(6, 6)::Identity() * 0.0001;
    P_init_output.block<6, 6>(24, 24) = MD(6, 6)::Identity() * 0.001;
    kf_output.change_P(P_init_output);
    Eigen::Matrix<double, 30, 30> Q_output = process_noise_cov_output();
    /*** debug record ***/
    FILE* fp;
    string pos_log_dir = root_dir + "/Log/pos_log.txt";
    fp = fopen(pos_log_dir.c_str(), "w");

    ofstream fout_out, fout_imu_pbp;
    fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), ios::out);
    fout_imu_pbp.open(DEBUG_FILE_DIR("imu_pbp.txt"), ios::out);
    if (fout_out && fout_imu_pbp)
        cout << "~~~~" << ROOT_DIR << " file opened" << endl;
    else
        cout << "~~~~" << ROOT_DIR << " doesn't exist" << endl;

    /*** ROS subscribe initialization ***/
    ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
    ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
    ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered", 100000);
    ros::Publisher pubLaserCloudFullRes_body = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered_body", 100000);
    ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>("/cloud_effected", 100000);
    ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/Laser_map", 100000);
    ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 100000);
    ros::Publisher pubPath = nh.advertise<nav_msgs::Path>("/path", 100000);
    ros::Publisher plane_pub = nh.advertise<visualization_msgs::Marker>("/planner_normal", 1000);
    //------------------------------------------------------------------------------------------------------
    signal(SIGINT, SigHandle);
    ros::Rate rate(5000);
    bool status = ros::ok();
    while (status) {
        if (flg_exit) break;
        ros::spinOnce();
        if (sync_packages(Measures)) {
            if (flg_first_scan) {
                first_lidar_time = Measures.lidar_beg_time;
                flg_first_scan = false;
                cout << "first lidar time" << first_lidar_time << endl;
            }

            if (flg_reset) {
                ROS_WARN("reset when rosbag play back");
                p_imu->Reset();
                flg_reset = false;
                continue;
            }
            double t0, t1, t2, t3, t4, t5, match_start, solve_start;
            match_time = update_time = 0;
            t0 = omp_get_wtime();
            // IMU初始化
            p_imu->Process(Measures, feats_undistort);

            if (feats_undistort->empty() || feats_undistort == NULL) {
                continue;
            }
            // 重力估计,只执行一次
            if (imu_en) {
                // 默认gravity_align_为false,但通过Process转true, 所以下面代码只执行一次估计
                if (!p_imu->gravity_align_) {
                    cout << "g_align ";
                    // 遍历, 直到找到一个 时间戳大于激光雷达测量开始时间lidar_beg_time
                    while (Measures.lidar_beg_time > imu_next.header.stamp.toSec()) {
                        // imu_last 和 imu_next 分别保存了上一帧和下一帧 IMU 数据
                        imu_last = imu_next;
                        imu_next = *(imu_deque.front());
                        imu_deque.pop_front();
                    }
                    // 计算重力矢量估计值: mean_acc乘以重力加速度常数 G_m_s2，除以加速度范数acc_norm, 将结果分别赋值，注意方向取反
                    state_out.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm;
                    state_out.acc = p_imu->mean_acc * G_m_s2 / acc_norm;
                    // 如果启用了重力对齐, 一般启用
                    if (gravity_align) {
                        Eigen::Matrix3d rot_init;
                        p_imu->gravity_ << VEC_FROM_ARRAY(gravity);  // 格式转换
                        // 基于输入的重力矢量，计算初始旋转矩阵 rot_init
                        p_imu->Set_init(state_out.gravity, rot_init);
                        state_out.rot.normalize();
                        // 计算并更新 state_out.acc，即将初始旋转矩阵的转置乘以重力矢量，再取反
                        state_out.acc = -rot_init.transpose() * state_out.gravity;
                    }
                    // 使用更新后的状态值 state_out 更新卡尔曼滤波器的状态
                    kf_output.change_x(state_out);
                }
            } else {
                if (!p_imu->gravity_align_) {
                    state_out.gravity << VEC_FROM_ARRAY(gravity_init);
                    state_out.acc << VEC_FROM_ARRAY(gravity_init);
                    state_out.acc *= -1;
                }
            }

            /*** Segment the map in lidar FOV ***/
            lasermap_fov_segment();
            /*** downsample the feature points in a scan ***/
            t1 = omp_get_wtime();
            // 降采样,默认为true
            if (space_down_sample) {
                // 输入点云为 feats_undistort，滤波结果为 feats_down_body
                downSizeFilterSurf.setInputCloud(feats_undistort);
                downSizeFilterSurf.filter(*feats_down_body);
                // 根据时间对点云进行排序
                sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list);
            } else {
                feats_down_body = Measures.lidar;
                sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list);
            }
            // 进行时间压缩处理，并将结果赋值给 time_seq
            time_seq = time_compressing<int>(feats_down_body);
            feats_down_size = feats_down_body->points.size();

            /*** initialize the map kdtree ***/
            if (!init_map) {
                if (ikdtree.Root_Node == nullptr)  //
                                                   // if(feats_down_size > 5)
                {
                    ikdtree.set_downsample_param(filter_size_map_min);
                }

                feats_down_world->resize(feats_down_size);
                for (int i = 0; i < feats_down_size; i++) {
                    pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
                }
                for (size_t i = 0; i < feats_down_world->size(); i++) {
                    init_feats_world->points.emplace_back(feats_down_world->points[i]);
                }
                if (init_feats_world->size() < init_map_size) continue;
                ikdtree.Build(init_feats_world->points);
                init_map = true;
                publish_init_kdtree(pubLaserCloudMap);  //(pubLaserCloudFullRes);
                continue;
            }

            /*** ICP and Kalman filter update ***/
            t2 = omp_get_wtime();
            normvec->resize(feats_down_size);
            feats_down_world->resize(feats_down_size);
            Nearest_Points.resize(feats_down_size);
            /*** iterated state estimation ***/
            crossmat_list.reserve(feats_down_size);
            pbody_list.reserve(feats_down_size);
            // 格式转换 与 计算反对称矩阵
            for (size_t i = 0; i < feats_down_body->size(); i++) {
                // 点云格式转换,并存到pbody_list
                V3D point_this(feats_down_body->points[i].x, feats_down_body->points[i].y, feats_down_body->points[i].z);
                pbody_list[i] = point_this;
                // 用固定外参对点云进行旋转平移 (来自配置文件)
                point_this = Lidar_R_wrt_IMU * point_this + Lidar_T_wrt_IMU;
                // 计算point_this的反对称矩阵（也称为叉乘矩阵）,添加到crossmat_list中
                M3D point_crossmat;
                point_crossmat << SKEW_SYM_MATRX(point_this);
                crossmat_list[i] = point_crossmat;
            }

            // 逐点更新,通过与IMU数据进行融合，对当前点云进行运动补偿
            bool imu_upda_cov = false;
            effct_feat_num = 0;
            abs_z = 0.0;
            /**** point by point update ****/
            // 初始化时间
            double pcl_beg_time = Measures.lidar_beg_time;
            idx = -1;  // 修正
            // 遍历时间分组 , 每组只更新第一个点,绝大多数组基本只有一个点, 按时间排序:EKF的要求
            for (k = 0; k < time_seq.size(); k++) {
                // 是具体一个点, 每组的第一个点
                PointType& point_body = feats_down_body->points[idx + time_seq[k]];
                // 表示当前时间。用于判断是否有新的 IMU 数据到达
                time_current = point_body.curvature / 1000.0 + pcl_beg_time;

                if (is_first_frame) {
                    if (imu_en) {
                        // 如果是第一帧且启用IMU，将IMU的线性加速度和角速度赋值给acc_avr和angvel_avr
                        while (time_current > imu_next.header.stamp.toSec()) {
                            imu_last = imu_next;
                            imu_next = *(imu_deque.front());
                            imu_deque.pop_front();
                            // imu_deque.pop();
                        }

                        angvel_avr << imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
                        acc_avr << imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
                    }
                    is_first_frame = false;
                    imu_upda_cov = true;
                    time_update_last = time_current;
                    time_predict_last_const = time_current;
                }
                // IMU 状态更新
                if (imu_en) {
                    // 检查是否有新IMU数据到来,即新的 IMU 数据的时间戳是否大于当前时间
                    bool imu_comes = time_current > imu_next.header.stamp.toSec();
                    while (imu_comes) {
                        // 如果有新数据，则需要进行协方差更新
                        imu_upda_cov = true;
                        // 提取新的IMU线性加速度和角速度
                        angvel_avr << imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
                        acc_avr << imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z;

                        /*** covariance update ***/
                        imu_last = imu_next;
                        imu_next = *(imu_deque.front());
                        imu_deque.pop_front();
                        // 计算当前点的时间与上一次预测时间的差值dt，并根据时间差进行状态预测,(time_predict_last_const表示上一次执行状态预测的时间)
                        double dt = imu_last.header.stamp.toSec() - time_predict_last_const;
                        // 新 IMU 到达时，只简单更新了一下状态 (通过input_in,即IMU的acc和gyro,更新x_)
                        kf_output.predict(dt, Q_output, input_in, true, false);
                        time_predict_last_const = imu_last.header.stamp.toSec();  // big problem ??????
                        imu_comes = time_current > imu_next.header.stamp.toSec();
                        // if (!imu_comes)
                        {
                            // 计算协方差更新时间差dt_cov
                            double dt_cov = imu_last.header.stamp.toSec() - time_update_last;
                            // 如果dt_cov大于0, 表示需要执行协方差更新
                            if (dt_cov > 0.0) {
                                // 上一次执行协方差更新的时间
                                time_update_last = imu_last.header.stamp.toSec();
                                // 新 IMU 到达后，真正的协方差预测步骤, 更新协方差矩阵，状态预测已经是最新
                                kf_output.predict(dt_cov, Q_output, input_in, false, true);
                                // IMU 状态更新
                                kf_output.update_iterated_dyn_share_IMU();
                            }
                        }
                    }
                }
                double dt = time_current - time_predict_last_const;
                // 默认不执行
                if (!prop_at_freq_of_imu) {
                    double dt_cov = time_current - time_update_last;
                    if (dt_cov > 0.0) {
                        kf_output.predict(dt_cov, Q_output, input_in, false, true);
                        time_update_last = time_current;
                    }
                }
                // IMU结束后，对 EKF 进行一次额外的状态预测，以便在下一次 保持状态估计的实时性。只简单更新了一下状态
                // 是否是Lidar的预测
                kf_output.predict(dt, Q_output, input_in, true, false);
                time_predict_last_const = time_current;
                double t_update_start = omp_get_wtime();
                // 例外情况处理, 点太少
                if (feats_down_size < 1) {
                    ROS_WARN("No point, skip this scan!\n");
                    idx += time_seq[k];
                    continue;
                }
                // Lidar 状态更新
                if (!kf_output.update_iterated_dyn_share_modified()) {
                    // 下一组
                    idx += time_seq[k];
                    continue;
                }

                // 默认true
                if (prop_at_freq_of_imu) {
                    double dt_cov = time_current - time_update_last;
                    if (!imu_en && (dt_cov >= imu_time_inte))  // (point_cov_not_prop && imu_prop_cov)
                    {
                        kf_output.predict(dt_cov, Q_output, input_in, false, true);
                        imu_upda_cov = false;
                        time_update_last = time_current;
                    }
                }
                // 一般false, pub全部里程计
                if (publish_odometry_without_downsample) {
                    publish_odometry(pubOdomAftMapped);
                    if (runtime_pos_log) {
                        state_out = kf_output.x_;
                        euler_cur = SO3ToEuler(state_out.rot);
                        fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_out.pos.transpose()
                                 << " " << state_out.vel.transpose() << " " << state_out.omg.transpose() << " " << state_out.acc.transpose() << " "
                                 << state_out.gravity.transpose() << " " << state_out.bg.transpose() << " " << state_out.ba.transpose() << " "
                                 << feats_undistort->points.size() << endl;
                    }
                }
                // 遍历该组的点, 逐个转换
                for (int j = 0; j < time_seq[k]; j++) {
                    PointType& point_body_j = feats_down_body->points[idx + j + 1];
                    PointType& point_world_j = feats_down_world->points[idx + j + 1];
                    pointBodyToWorld(&point_body_j, &point_world_j);
                }

                update_time += omp_get_wtime() - t_update_start;
                idx += time_seq[k];  // 下一组
            }

            // 矩阵的迹, 对角线元素之和
            // double trace = P_.trace();
            // // 矩阵的行列式
            // double det = P_.determinant();

            t3 = omp_get_wtime();

            if (EN_GTSAM) {
                // 输出此次配准效果
                getCurPose(kf_output.x_);  //   更新transformTobeMapped

                // back end
                // 1.计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
                // 2.添加激光里程计因子、GPS因子、闭环因子
                // 3.执行因子图优化
                // 4.得到当前帧优化后的位姿，位姿协方差
                // 5.添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
                saveKeyFramesAndFactor();
                // 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹， 重构ikdtree
                correctPoses();
            }

            if (!publish_odometry_without_downsample) {
                publish_odometry(pubOdomAftMapped);
            }

            /*** add the feature points to map kdtree ***/

            if (feats_down_size > 4) {
                map_incremental();
            }
            t5 = omp_get_wtime();
            /******* Publish points *******/
            if (path_en) {
                publish_path(pubPath);
                if (EN_GTSAM) {
                    publish_path_update(pubPathUpdate);  //   发布经过isam2优化后的路径
                    static int jjj = 0;
                    jjj++;
                    if (jjj % 100 == 0) publishGlobalMap();  //  发布局部点云特征地图
                }
            }

            if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFullRes);           // 默认true,只发送当前扫描转换的点云
            if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFullRes_body);  // 默认false

            /*** Debug variables Logging ***/
            if (runtime_pos_log) {
                frame_num++;
                aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;
                { aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + update_time / frame_num; }
                aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num;
                T1[time_log_counter] = Measures.lidar_beg_time;
                s_plot[time_log_counter] = t5 - t0;
                s_plot2[time_log_counter] = feats_undistort->points.size();
                s_plot3[time_log_counter] = aver_time_consu;
                time_log_counter++;

                // printf("[mapping (ms)]: IMU+Map+Ds+match: %0.3f ICP: %0.3f ave icp: %0.3f ave total: %0.3f \n",
                //     (t1-t0+aver_time_match)*1000.0,(t3-t1)*1000.0,aver_time_icp*1000.0, aver_time_consu*1000.0);
                if (!publish_odometry_without_downsample) {
                    state_out = kf_output.x_;
                    euler_cur = SO3ToEuler(state_out.rot);
                    fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_out.pos.transpose()
                             << " " << state_out.vel.transpose() << " " << state_out.omg.transpose() << " " << state_out.acc.transpose() << " "
                             << state_out.gravity.transpose() << " " << state_out.bg.transpose() << " " << state_out.ba.transpose() << " "
                             << feats_undistort->points.size() << endl;
                }
                dump_lio_state_to_log(fp);
            }
        }
        status = ros::ok();
        rate.sleep();
    }

    startFlag = false;
    if (EN_LOOP) {
        loopthread.join();  //  分离线程
    }
    // DataRecorder::DumpIntoFile(std::string(ROOT_DIR)+"Log/data.txt");
    saveTrajectoryTUM(cloudKeyPoses6D, "optimized_pose_tum.txt");
    saveTrajectoryTUM(fastlio_unoptimized_cloudKeyPoses6D, "without_optimized_pose_tum.txt");

    //--------------------------save map-----------------------------------
    /* 1. make sure you have enough memories
    /* 2. noted that pcd save will influence the real-time performences **/
    if (pcl_wait_save->size() > 0 && pcd_save_en) {
        string file_name = string("scans.pcd");
        string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
        pcl::PCDWriter pcd_writer;
        pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
    }
    fout_out.close();
    fout_imu_pbp.close();

    return 0;
}
